import java.util.*;

class TimeWarp
{
    boolean debug = false;
    ArrayList<Double> b2;
    double[] b;
    double[] a;
    double[][] graph;
    double dp = 0.0;

    double[][][] preCalcs; //i, j, {0,1,2} for left, up and diag respectively

    int score = 0;
    int stretchDistance = 50;//PARAMETER: distance between samples for stretch
    int bendDistance = 3;//3;//1;//25;//PARAMETER: distance between samples for bend
    int stretchWindow = 100;//PARAMETER: sliding window avg window for stretch
    int bendWindow = 6;//3;//1;//50; //PARAMETER: sliding window avg window for bend

    double stretchScale = 2.0;//PARAMETER: Scales importance of stretching.
    double bendScale = 0.08;//7.0;//PARAMETER: Scales importance of bending.
    double insertOffset = 0.0;

    double distPunish = 0.0;
    public double limit = 9999999.0;
    private boolean didDel = false;
    private boolean didInsert = false;
    private double delTotal = 0.0;
    private int delCount = 0;
    private double[] path;
    public Matrix costMatrix;
    public MatLabArray pathArray;
    public double[] slWinABendVal;
    public double[] slWinBBendVal;
    public double[] slWinAStretchVal;
    public double[] slWinBStretchVal;

    //warps b to a, i.e. changes b
    public TimeWarp(double[] a1, double[] b1)
    {
        score = 0;
        if (a1.length > b1.length)
        {
            a = trim(a1, b1.length);
            b = b1;
        }
        else
        {
            b = trim(b1, a1.length);
            a = a1;
        }
        graph = new double[a.length][a.length];
        preCalcs = new double[a.length][a.length][3];

        initSlWin();

        b2 = new ArrayList<Double>();
        path = new double[a.length];
        System.out.println("************************** Updating costs... ************************");
        updateCosts();

        costMatrix = new Matrix(graph);
        costMatrix.setName("costs");

   //-----------------------------------DEBUG
   //     costMatrix.matLabToFile("cost.m");
   //----------------------------------------

        System.out.println("***************************** Warping... ****************************");
        warp();
        System.out.println("******************************** Done. ******************************");
    }

    double[] slidingWindowArray(double[] samples, int width)
    {
        double[] averages = new double[samples.length];
        int offset = (width-1) / 2;
        double total = samples[0];//0.0;

        for (int index = 0; index < samples.length; index++)
        {
    
            int lIndex = index - offset;
            int rIndex = index + offset;
    
            if (lIndex < 0)
            {
                lIndex = 0;
            }
            total-= samples[lIndex];

            if (rIndex < 0)
                rIndex = 0;
    
            if (rIndex >= samples.length)
            {
                rIndex = (samples.length - 1);
            }
            total+= samples[rIndex];

            if (lIndex >= samples.length)
                lIndex = (samples.length - 1);

            double divisor = (rIndex - lIndex)+1.0;

            if (divisor < 1.0)
                divisor = 1.0;

            averages[index] = (total / divisor);
        }
        return averages;
    }

    void initSlWin()
    {
/*        slWinABendVal = new double[graph.length];
        slWinBBendVal = new double[graph.length];
        slWinAStretchVal = new double[graph.length];
        slWinBStretchVal = new double[graph.length];

        for (int i = 0; i < graph.length; i ++)
        {
            slWinABendVal[i] = slWinABend(i);
            slWinBBendVal[i] = slWinBBend(i);
            slWinAStretchVal[i] = slWinAStretch(i);
            slWinBStretchVal[i] = slWinBStretch(i);
        }*/
        slWinABendVal = slidingWindowArray(a,bendWindow);
        slWinBBendVal = slidingWindowArray(b,bendWindow);
        slWinAStretchVal = slidingWindowArray(a,stretchWindow);
        slWinBStretchVal = slidingWindowArray(b,stretchWindow);
    }

    public double[] getWave()
    {
        double[] arr = new double[b2.size()];
        for (int i = 0; i < b2.size(); i++)
        {
            arr[i] = b2.get(i).doubleValue();
        }
        return trim(arr,b.length);
    }

    private void warp()
    {
        int i = graph.length - 1;
        int j = i;
        didDel = false;
        delCount = 0;
        delTotal = 0.0;
        while (i > 0 || j > 0)
        {
            addPathRecord(i,j);
            if (i > 0 && j > 0)
            {
                if ((graph[i][j] - graph[i-1][j-1]) == (preCalcs[i][j][2] - graph[i-1][j-1])/*local diag*/)
                {
                    doPend(0);
                    didInsert = false;
                    substitute(i);
                    i--;
                    j--;
                    continue;
                }
                
                if((graph[i][j] - graph[i][j-1]) == (preCalcs[i][j][1] - graph[i][j-1])/*local up*/)
                {
                    doPend(0);
                    insert(i,j);
                    j--;
                    continue;
                }
                if((graph[i][j] - graph[i-1][j]) == (preCalcs[i][j][0] - graph[i-1][j])/*local left*/)
                {
                    didInsert = false;
                    pendDel(i);
                    i--;
                    continue;
                }
            }
            if (j == 0)
            {
                didInsert = false;
                pendDel(i);
                i--;
                continue;
            }
            if (i == 0)
            {
                doPend(i);
                insert(i,j);
                j--;
            }
        }
        doPend(0);
        substitute(0);
        matLabPath("path.array");
    }

    void addPathRecord(int i, int j)
    {
        path[i] = j;
    }

    void matLabPath(String filename)
    {
        pathArray = new MatLabArray("path",path);
    }

    void doPend(int i)
    {
        if (didDel == true)
        {
            delete(i);
            double avg =delTotal/(double)(delCount);
            b2.add(0,avg);
            delTotal = 0.0;
            delCount = 0;
            didDel = false;
            if (debug == true) System.out.println("Performed pending delete");
        }
    }

    void pendDel(int i)
    {
        delCount++;
        delTotal+= b[i];
        didDel = true;
        if (debug == true) System.out.println("Delete pending, data copied...");
    }

    private void substitute(int i)
    {
        b2.add(0, b[i]);
        if (debug == true) System.out.println("Substitute");
    }

    private void insert(int i, int j)
    {
        if (b2.size() > 0)
            b2.add(0,b2.get(0));
        else
            b2.add(0, b[i]);

        if (debug == true) System.out.println("Insert");
    }

    private void delete(int i)
    {
        if (debug == true) System.out.println("Delete");
    }

    //Calculates cost of stretching between data points. Alter k to affect
    //stuff
    private double stretchCost(int i0, int j0, int i1, int j1)
    {
        double k = 100.0; //PARAMETER: See declaration of v below.
        double expo = 1.0; //PARAMETER: Exponent of distance measure, see v.
        double con = 0.5;

        int delta1 = (i1 - i0) * (stretchDistance);
        int delta2 = (j1 - j0) * (stretchDistance);
        i0-= delta1;
        j0-= delta2;

        if (i0 < 0) i0 = 0; if (i1 < 0) i1 = 0;
        if (j0 < 0) j0 = 0; if (j1 < 0) j1 = 0;

        double jDiff = (slWinAStretchVal[(j1)] - slWinAStretchVal[(j0)]);
        double iDiff = (slWinBStretchVal[(i1)] - slWinBStretchVal[(i0)]);
        double v = stretchScale * Math.pow(k * Math.abs( jDiff  - iDiff ), expo);

        double res = v;

        return v;
    }

    //Calculates cost of bending between data points. This is very fast, rough
    //approximation of a bend only and not directly equivalent to the angle
    //between the points
    private double bendCost(int i0, int j0, int i1, int j1, int i2, int j2)
    {
        int delta1 = (i1 - i0) * bendDistance;
        int delta2 = (j1 - j0) * bendDistance;
        i1-= delta1;
        j1-= delta2;
        i0-= delta1*2;
       j0-= delta2*2;

        if (i0 < 0) i0 = 0; if (i1 < 0) i1 = 0;
        if (j0 < 0) j0 = 0; if (j1 < 0) j1 = 0;

        double iASq = Math.abs((Math.pow(bendDistance*2,2.0)) + Math.pow(slWinBBendVal[(i2)]-slWinBBendVal[(i0)],2.0));
        double iBSq = Math.abs((Math.pow(bendDistance,2.0)) + Math.pow(slWinBBendVal[(i2)]-slWinBBendVal[(i1)],2.0));
        double iCSq = Math.abs((Math.pow(bendDistance,2.0)) + Math.pow(slWinBBendVal[(i1)]-slWinBBendVal[(i0)],2.0));

        double iB = Math.sqrt(iBSq);
        double iC = Math.sqrt(iCSq);

        double cosV1 = (iBSq + iCSq - iASq) / (2.0 * iB * iC);
        if (cosV1 > 1.0) cosV1 = 1.0; if (cosV1 < -1.0) cosV1 = -1.0;

        double iAng = Math.acos( cosV1 );

        double jASq = Math.abs((Math.pow(bendDistance*2,2.0)) + Math.pow(slWinABendVal[(j2)]-slWinABendVal[(j0)],2.0));
        double jBSq = Math.abs((Math.pow(bendDistance,2.0)) + Math.pow(slWinABendVal[(j2)]-slWinABendVal[(j1)],2.0));
        double jCSq = Math.abs((Math.pow(bendDistance,2.0)) + Math.pow(slWinABendVal[(j1)]-slWinABendVal[(j0)],2.0));

        double jB = Math.sqrt(jBSq);
        double jC = Math.sqrt(jCSq);

        double cosV2 = (jBSq + jCSq - jASq) / (2.0 * jB * jC);
        if (cosV2 > 1.0) cosV2 = 1.0; if (cosV2 < -1.0) cosV2 = -1.0;

        double jAng = Math.acos( cosV2 );

        boolean i1Max = ((slWinBBendVal[(i1)] - slWinBBendVal[(i0)]) -
            (slWinBBendVal[(i2)] - slWinBBendVal[(i1)])) > 0;
        boolean j1Max = ((slWinABendVal[(j1)] - slWinABendVal[(j0)]) -
            (slWinABendVal[(j2)] - slWinABendVal[(j1)])) > 0;

       if (i1Max == j1Max)
        {
            return (bendScale * Math.abs( Math.toDegrees(iAng - jAng) ));
        }
        else
        {
            return (bendScale * (360.0 - Math.abs( Math.toDegrees(iAng - jAng) )));
        } 
    }

    double slWinABend(int index)
    {
        return slidingWindow(a,index,bendWindow);
    }

    double slWinBBend(int index)
    {
        return slidingWindow(b,index,bendWindow);
    }

    double slWinAStretch(int index)
    {
        return slidingWindow(a,index,stretchWindow);
    }

    double slWinBStretch(int index)
    {
        return slidingWindow(b,index,stretchWindow);
    }

    //Uses sliding window averaging to take a sample around a point
    double slidingWindow(double[] samples, int index, int width)
    {
        int offset = (width-1) / 2;
        double total = 0.0;

        int lIndex = index - offset;
        int rIndex = index + offset;

        if (lIndex < 0)
            lIndex = 0;
        if (rIndex < 0)
            rIndex = 0;

        if (rIndex >= samples.length)
            rIndex = (samples.length - 1);
        if (lIndex >= samples.length)
            lIndex = (samples.length - 1);

        for (int i = lIndex; i <= rIndex; i++)
        {
            total+= samples[i];
        }

        double divisor = (rIndex - lIndex)+1.0;
        if (divisor < 1.0)
            divisor = 1.0;

        return ( total / divisor );
    }

    double constrain(int i, int j)
    {
        double c = (double)Math.abs((double)i - (double)j) * distPunish;
        return c*c;
    }

    private double leftWork(int i, int j)
    {
        if (i < 0) i = 0; if (j < 0) j = 0;
        if (i == 0)
        {
            preCalcs[i][j][0] = this.limit;
        }
        else
        {
            preCalcs[i][j][0] = ( stretchCost(i-1,j,i,j) + 
            Math.min( preCalcs[i-1][j][0] + bendCost(i-2,j,i-1,j,i,j),
            preCalcs[i-1][j][2] + bendCost(i-2,j-1,i-1,j,i,j) ) )
            + constrain(i,j);
        }

        return preCalcs[i][j][0];
    }

    private double upWork(int i, int j)
    {
        if (i < 0) i = 0; if (j < 0) j = 0;
        if (j == 0)
        {
            preCalcs[i][j][1] = this.limit;
        }
        else
        {
            preCalcs[i][j][1] = ( stretchCost(i,j-1,i,j) +
            Math.min( preCalcs[i][j-1][1]  + bendCost(i,j-2,i,j-1,i,j) ,
            preCalcs[i][j-1][2]  + bendCost(i-1,j-2,i,j-1,i,j)  ) )
            + constrain(i,j);
        }
        return preCalcs[i][j][1];
    }

    private double diagWork(int i, int j)
    {
        if (i < 0) i = 0; if (j < 0) j = 0;
        if ((i == 0) || (j == 0))
        {
             if (!(i == 0 && j == 0))
             {
                 preCalcs[i][j][2] = this.limit;
             }
             else
             {
                 preCalcs[i][j][2] = 0.0;
             }
        }
        else
        {
            preCalcs[i][j][2] = ( stretchCost(i-1,j-1,i,j) +
            Math.min( preCalcs[i-1][j-1][1]  + bendCost(i-1,j-2,i-1,j-1,i,j) ,
                Math.min( preCalcs[i-1][j-1][2]  + bendCost(i-2,j-2,i-1,j-1,i,j) ,
                preCalcs[i-1][j-1][0]  + bendCost(i-2,j-1,i-1,j-1,i,j) ) ) )
                 + constrain(i,j);
        }

        return preCalcs[i][j][2];

    }

    private double localCost(int i0, int j0, int i1, int j1)
    {
        return stretchCost(i0, j0, i1, j1);
    }

    private double globalCost(int i, int j)
    {
        double v = Math.min( leftWork(i,j) ,
            Math.min( upWork(i,j), diagWork(i,j) ) );
        if (Double.isNaN(v))
        {
            System.out.println("GLOBALCOST IS NaN! i = "+i+", j = "+j);
            System.out.println("leftWork = "+leftWork(i,j));
            System.out.println("upWork = "+upWork(i,j));
            System.out.println("diagWork = "+diagWork(i,j));
            System.exit(1);
        }
        return v;
    }

    private void updateCosts()
    {
        int i = 0;
        int j = 0;
        for (j = 0; j < graph.length; j++)
        {
            for (i = 0; i < graph.length; i++)
            {
                graph[i][j] = globalCost(i, j);
            }
        }
        //---- DEBUGGING, PRINT FINAL COST
        System.out.println("***** GLOBALLY OPTIMISED COST FOR THIS WARP : "+
            graph[i-1][j-1]+" *****");
        //----
    }
    
    private double[] trim(double[] in, int size)
    {
        double[] out = new double[size];
        for (int i = 0; i < size; i++)
            out[i] = in[i];
        return out;
    }
}
